import networkx as nx
import heapq
from . import utils


class AstarPathPlannerV1:
    def __init__(self,file_path):
        nodes, edges = utils.Utils.read_data(file_path)
        # 构建图
        self.graph = nx.DiGraph()
        for i in range(len(nodes)):
            self.graph.add_node(str(nodes.loc[i,"id"]),name=nodes.loc[i,"name"],lat=nodes.loc[i,"lat"],lon=nodes.loc[i,"lon"],g=0,h=0,f=0,parent=None)
        for i in range(len(edges)):
            self.graph.add_edge(str(edges.loc[i,"start_id"]),str(edges.loc[i,"end_id"]),id=str(edges.loc[i,"id"]),len=edges.loc[i,"len"])

    def plan_path(self,start_id:str,end_id:str)->list:
        open_list = []
        close_list = []
        heapq.heappush(open_list, start_id)

        while open_list:
            current_node_id = heapq.heappop(open_list)
            close_list.append(current_node_id)

            if current_node_id==end_id:
                path = []
                # 到达终点，找到一条路径
                while self.graph.nodes[current_node_id]["parent"] is not None:
                    path.append(current_node_id)
                    current_node_id = self.graph.nodes[current_node_id]["parent"]
                return path[::-1]

            neighbor_ids = self.graph.neighbors(current_node_id)
            for neighbor_id in neighbor_ids:
                if neighbor_id in close_list:
                    continue
                self.graph.nodes[neighbor_id]["g"] = self.graph.nodes[current_node_id]["g"]+self.graph[current_node_id][neighbor_id]["len"]
                self.graph.nodes[neighbor_id]["h"] = abs(self.graph.nodes[neighbor_id]["lat"]-self.graph.nodes[end_id]["lat"])+abs(self.graph.nodes[neighbor_id]["lon"]-self.graph.nodes[end_id]["lon"])
                self.graph.nodes[neighbor_id]["f"] = self.graph.nodes[neighbor_id]["g"]+self.graph.nodes[neighbor_id]["h"]
                self.graph.nodes[neighbor_id]["parent"] = current_node_id

                if any(node_id == neighbor_id and self.graph.nodes[node_id]["f"]<self.graph.nodes[neighbor_id]["f"] for node_id in open_list):
                    continue
                heapq.heappush(open_list,neighbor_id)

        return None


# astar_planner = AstarPathPlannerV1("./data.xlsx")
# path = astar_planner.plan_path("1","2")